// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ObjectDetectionVisualIntegration.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Manipulation
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Threading;

    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Hardware.Runtime;
    using Microsoft.Robotics.Hardware.Runtime.Robotiq;
    using Microsoft.Robotics.PhysicalObjects;
    using Microsoft.Robotics.Manipulation.PicknPlace;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.ObjectDetection;
    using Microsoft.Robotics.Vision.Runtime;

    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Hardware.KukaLBR.KukaLBR4Tests;
    using Test.Robotics.Runtime;

    /// <summary>
    /// Test for loading detector configuration
    /// </summary>
    [TestClass]
    public class ObjectDetectionVisualIntegrationTest
    {
        /// <summary>
        /// Pipeline gripper host
        /// </summary>
        private IAgentHost gripperHost;

        /// <summary>
        /// Pipeline timer host
        /// </summary>
        private IAgentHost manipTimerHost;

        /// <summary>
        /// Pipeline timer agent
        /// </summary>
        private TimerAgent manipTimerAgent;

        /// <summary>
        /// Consumer of pick and place status messages from visual integrator
        /// </summary>
        private MessageCollectorAgent<PicknPlaceStatusMessage> PicknPlaceStatusConsumerAgent;

        private IAgentHost PicknPlaceConsumerHost;

        /// <summary>
        /// Pick and place host
        /// </summary>
        private IAgentHost pickNPlaceSimulatorHost;

        /// <summary>
        /// Pipeline pick and place message producer
        /// </summary>
        private IForwarder<PickAndMoveMessage> pickMoveMessageInjector;
        
        /// <summary>
        /// Pipeline pick and place simulator
        /// </summary>
        private IForwarder<PlaceAndMoveMessage> placeMoveMessageInjector;

        /// <summary>
        /// Injector of object detection messages
        /// </summary>
        private MessageInjectorAgent<KinectObjectDetectorResultIRMessage> objectDetectorAgent;

        /// <summary>
        /// Pipeline object detector result producer
        /// </summary>
        private IAgentHost objectDetectorHost;

        /// <summary>
        /// Visual integratioon test agent
        /// </summary>
        private ObjectDetectorPicknPlaceIntegratorAgent visualIntegrationAgent;

        /// <summary>
        /// Pipeline integration host
        /// </summary>
        private IAgentHost visualIntegrationHost;

        /// <summary>
        /// Test Visual integration to report when visual detectection
        /// estimates match expected poses and to report the different
        /// types of errocorrectly
        /// </summary>
        [TestMethod]
        public void ObjectDetectionState()
        {
            ObjectDetectorPicknPlaceIntegratorState state = new ObjectDetectorPicknPlaceIntegratorState(1, 3, null, null);
            ImageFrameDepth depthImage = new ImageFrameDepth(2,3);
            ImageFrame<ushort> irImage = new ImageFrame<ushort>(2,3);
            PhysicalObjectDescription canTomato = new PhysicalObjectDescription(PhysicalObjectShape.Cylinder, 0.075, 0.110, 0.075, 411.0, "TomatoCan");
            PhysicalObjectDescription boxSoap = new PhysicalObjectDescription(PhysicalObjectShape.BoxRegular, 0.097, 0.186, 0.063, 678.0, "SoapBox");
            List<string> expectedObjects = new List<string>() {"TomatoCan", "TomatoCan", "SoapBox"};

            PickAndMove[] pickMoveRequests = new PickAndMove[] { 
                new PickAndMove(canTomato, new Pose(new Vector3(0.1, 0.2, 0.3)), new Pose(), PicknPlaceModeEnum.VisualObjectDetection),
                new PickAndMove(canTomato, new Pose(new Vector3(0.2, 0.4, 0.6)), new Pose(), PicknPlaceModeEnum.VisualObjectDetection),
                new PickAndMove(boxSoap, new Pose(new Vector3(0.3, 0.6, 0.8)), new Pose(), PicknPlaceModeEnum.VisualObjectDetection)};

            Pose cameraPose = new Pose();
            ObjectPoseEstimate canBelowEstimate = new ObjectPoseEstimate(cameraPose, 0.1, 0.5, canTomato);
            ObjectPoseEstimate can = new ObjectPoseEstimate(cameraPose, 0.6, 0.5, canTomato);
            ObjectPoseEstimate box = new ObjectPoseEstimate(cameraPose, 0.9, 0.9, boxSoap);

            ObjectPoseEstimate[] estimates = new ObjectPoseEstimate[] {
                                   new ObjectPoseEstimate(canBelowEstimate, new Pose(new Vector3(0.3, 0.4, 0.7))),
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.102, 0.205, 0.295))),
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.204, 0.405, 0.603))),
                                   new ObjectPoseEstimate(box, new Pose(new Vector3(0.305, 0.598, 0.795)))};

            // Condition 1 Pose estimates that match teh pick request
            PickAndMove[] pickAndMovesFound;
            KinectObjectDetectorResultIRMessage detectMessage = new KinectObjectDetectorResultIRMessage(AgentMessage.GetCurrentTime(), depthImage, irImage, estimates);
            state.NewPickMessageReceived(pickMoveRequests, detectMessage.OriginatingTime+ 10, expectedObjects);
            bool isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            state.NewPickMessageReceived(pickMoveRequests, detectMessage.OriginatingTime - 10, expectedObjects);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess);
            Assert.AreEqual(pickAndMovesFound.Length, 3);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickInProgress);

            // Condition 2: Pose estimates deviates from request sufficientlt to trigger an error
            state.NewPickMessageReceived(pickMoveRequests, 10, expectedObjects);
            estimates = new ObjectPoseEstimate[] {
                                   new ObjectPoseEstimate(canBelowEstimate, new Pose(new Vector3(0.3, 0.4, 0.7))),
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.102, 0.205, 0.295))),
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.204, 0.405, 0.603))),
                                   new ObjectPoseEstimate(box, new Pose(new Vector3(0.305, 0.586, 0.795)))};
            detectMessage = new KinectObjectDetectorResultIRMessage(AgentMessage.GetCurrentTime(), depthImage, irImage, estimates);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickRequestReceived);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickRequestReceived);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickFailLargeError);

            // Condition 3: Estimate confidence too low triggering a false negative determination
            state.NewPickMessageReceived(pickMoveRequests, 10, expectedObjects);
            estimates = new ObjectPoseEstimate[] {
                                   new ObjectPoseEstimate(canBelowEstimate, new Pose(new Vector3(0.3, 0.4, 0.7))),
                                   new ObjectPoseEstimate(canBelowEstimate, new Pose(new Vector3(0.102, 0.205, 0.295))),
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.204, 0.405, 0.603))),
                                   new ObjectPoseEstimate(box, new Pose(new Vector3(0.305, 0.596, 0.795)))};
            detectMessage = new KinectObjectDetectorResultIRMessage(AgentMessage.GetCurrentTime(), depthImage, irImage, estimates);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickRequestReceived);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickRequestReceived);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickFailFalseNegative);

            // Condition 4: Phantom detection triggers a false positive determination
            state.NewPickMessageReceived(pickMoveRequests, 10, expectedObjects);
            estimates = new ObjectPoseEstimate[] {
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.3, 0.4, 0.7))),
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.102, 0.205, 0.295))),
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.204, 0.405, 0.603))),
                                   new ObjectPoseEstimate(box, new Pose(new Vector3(0.305, 0.596, 0.795)))};
            detectMessage = new KinectObjectDetectorResultIRMessage(AgentMessage.GetCurrentTime(), depthImage, irImage, estimates);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickRequestReceived);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickRequestReceived);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickFailFalsePositive);

            // Condition 4; Muttiple errors. reports last error detected in this case large pose deviation
            state.NewPickMessageReceived(pickMoveRequests, 10, expectedObjects);
            estimates = new ObjectPoseEstimate[] {
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.3, 0.4, 0.7))),
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.102, 0.205, 0.295))),
                                   new ObjectPoseEstimate(can, new Pose(new Vector3(0.204, 0.405, 0.603))),
                                   new ObjectPoseEstimate(box, new Pose(new Vector3(0.305, 0.586, 0.795)))};
            detectMessage = new KinectObjectDetectorResultIRMessage(AgentMessage.GetCurrentTime(), depthImage, irImage, estimates);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickRequestReceived);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickRequestReceived);
            isSuccess = state.IsNewDectionSuccess(detectMessage, out pickAndMovesFound);
            Assert.IsTrue(isSuccess == false);
            Assert.IsTrue(state.LastPickStatus == PicknPlaceStatusEnum.PickFailLargeError);
        }

        /// <summary>
        /// Test the visual integration agent by setting up a pipeline
        /// and sending a few messages through it
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Unit")]
        [DeploymentItem("Microsoft.Robotics.Hardware.KukaAPI.dll")]
        [DeploymentItem("Microsoft.Robotics.Manipulation.ControllerStateMachine.dll")]
        [DeploymentItem("Microsoft.Robotics.Hardware.KukaLBR.KukaLBR4Controller.dll")]
        [DeploymentItem(@"Microsoft.Robotics.Vision.Native.dll")]
        public void ObjectDetectionVisualIntegrationPipelineRun()
        {
            try
            {
                this.CreateVisualIntegrationTestPipeline();
                Assert.IsNotNull(this.visualIntegrationAgent);
                Assert.IsNotNull(this.visualIntegrationAgent.State);

                PhysicalObjectDescription can = new PhysicalObjectDescription(PhysicalObjectShape.Cylinder, 0.075, 0.110, 0.075, 411.0, "canTest");
                Pose pickFrom = new Pose(new Vector3(0.3, -0.8, 0.1));
                Pose badpickFrom = new Pose(new Vector3(0.28, -0.8, 0.1));
                Pose placeTo = new Pose(new Vector3(-0.3, -0.7, 0));
                ObjectPoseEstimate goodCameraEstimate = new ObjectPoseEstimate(new Pose(), 0.95, 0.9, can);
                ObjectPoseEstimate badConfCameraEstimate = new ObjectPoseEstimate(new Pose(), 0.75, 0.9, can);

                PickAndMoveMessage pickMove = new PickAndMoveMessage(1, new PickAndMove[] { new PickAndMove(can, pickFrom, placeTo, PicknPlaceModeEnum.VisualObjectDetection) });
                PlaceAndMoveMessage placeMove = new PlaceAndMoveMessage(0, new PlaceAndMove[] { new PlaceAndMove(can, placeTo, pickFrom) });
                ObjectPoseEstimate goodEstimate = new ObjectPoseEstimate(goodCameraEstimate, pickFrom);
                ObjectPoseEstimate badConfEstimate = new ObjectPoseEstimate(badConfCameraEstimate, pickFrom);
                ObjectPoseEstimate badPoseEstimate = new ObjectPoseEstimate(goodCameraEstimate, badpickFrom);

                this.PicknPlaceStatusConsumerAgent.Reset();
                this.WaitAndCheckStatus(2, null, PicknPlaceStatusEnum.NotStarted, null);

                this.pickMoveMessageInjector.Post(pickMove);
                Thread.Sleep(20);
                this.WaitAndCheckStatus(20, null, PicknPlaceStatusEnum.PickRequestReceived, null);
                this.WaitAndCheckStatus(20, goodEstimate, PicknPlaceStatusEnum.PickInProgress, pickFrom);

                this.WaitAndCheckStatus(20, null, PicknPlaceStatusEnum.PickComplete, pickFrom);
                this.placeMoveMessageInjector.Post(placeMove);
                this.WaitAndCheckStatus(20, null, PicknPlaceStatusEnum.PlaceComplete, placeTo);

                this.pickMoveMessageInjector.Post(pickMove);
                Thread.Sleep(20);
                this.PostDetection(badConfEstimate);
                this.WaitAndCheckStatus(20, badConfEstimate, PicknPlaceStatusEnum.PickFailFalseNegative, null);

                this.pickMoveMessageInjector.Post(pickMove);
                Thread.Sleep(20);
                this.PostDetection(badPoseEstimate);
                this.WaitAndCheckStatus(20, badPoseEstimate, PicknPlaceStatusEnum.PickFailLargeError, null);
            }
            catch
            {
                throw;
            }
            finally
            {
                this.ShutDownPipeline();
            }
        }

        /// <summary>
        /// Waits for status and checks for expected status
        /// </summary>
        /// <param name="timeOutMsec">Time out in msec</param>
        /// <param name="expectedStatus">Expected pick and place status</param>
        /// <param name="expectedPose">Expected pick place pose</param>
        private void WaitAndCheckStatus(int timeOutSec, ObjectPoseEstimate estimate, PicknPlaceStatusEnum expectedStatus, Pose? expectedPose)
        {
            long timeOutTicks = Stopwatch.Frequency * timeOutSec;
            long elapsedTicks = 0;
            long startTicks = Stopwatch.GetTimestamp();
            PicknPlaceStatusMessage statusMsg = null; ;

            bool cont = true;
            do
            {
                this.PicknPlaceStatusConsumerAgent.Wait(10);

                if (this.PicknPlaceStatusConsumerAgent.Results.Count > 0)
                {
                    statusMsg = this.PicknPlaceStatusConsumerAgent.Results.Dequeue();

                    if (statusMsg.StatusEnum == expectedStatus)
                    {
                        cont = false;
                    }

                    this.PicknPlaceStatusConsumerAgent.Reset();
                }

                if (true == cont && null != estimate)
                {
                    this.PostDetection(estimate);
                }


                elapsedTicks = Stopwatch.GetTimestamp() - startTicks;
                cont &= elapsedTicks <= timeOutTicks;
            } while (true == cont);

            Assert.IsNotNull(statusMsg);
            Assert.AreEqual(statusMsg.StatusEnum, 
                expectedStatus,
                string.Format("Expected status {0} Found {1}", expectedStatus.ToString(), statusMsg.StatusEnum.ToString()));

            if (true == expectedPose.HasValue)
            {
                Assert.IsTrue(
                    statusMsg.PickPlacePose.Position == expectedPose.Value.Position,
                    string.Format("Expected position {0} Found {1}", expectedPose.Value.Position.ToString(), statusMsg.PickPlacePose.Position.ToString()));
            }
        }


        /// <summary>
        /// Post a object detection estimate
        /// </summary>
        /// <param name="estimate">Object detection estimate</param>
        private void PostDetection(ObjectPoseEstimate estimate)
        {
            KinectObjectDetectorResultIRMessage detect = new KinectObjectDetectorResultIRMessage(
                            AgentMessage.GetCurrentTime(),
                            new ImageFrameDepth(10, 10),
                            new ImageFrame<ushort>(10, 10),
                            new ObjectPoseEstimate[] { estimate });

            this.objectDetectorAgent.Publish(detect);
        }

        /// <summary>
        /// Set up a simple visual integration pipeline
        /// </summary>
        private void CreateVisualIntegrationTestPipeline()
        {
            string armAgentName = "armAgent";
            string handAgentName = "handAgent";
            string pickPlaceAgentName = "pickPlaceAgent";
            string integrationAgentName = "integrationAgent";
            string objectDetectorName = "objectDetector";
            double minConfidence = 0.9;
            int maxRetry = 2;
            int minDetectionCount = 1;

            FriSimulator testFri = new FriSimulator();
            ArmControlKuka armControl = new ArmControlKuka(testFri);

            // Create a pipeline and send a single Kinect message
            AgentLocator agentLocator = new AgentLocator();

            this.manipTimerAgent = new TimerAgent("ManipulationHeartbeatTimer", 50, 1);

            KukaLBRControlAgent armAgent = new KukaLBRControlAgent(
                        armAgentName,
                        this.manipTimerAgent,
                        armControl,
                        0.1,
                        0.1,
                        0.05,
                        false,
                        1);

            MockRobotiqSAgent robotiqAgent = new MockRobotiqSAgent(
                name: handAgentName,
                producer: this.manipTimerAgent);

            PicknPlaceAgentKukaRobotiq pickPlaceAgent = new PicknPlaceAgentKukaRobotiq(
                                            name: pickPlaceAgentName,
                                            producer: armAgent,
                                            armControlAgentName: armAgentName,
                                            handControlAgentName: handAgentName,
                                            handStatusProvider: handAgentName,
                                            armMoveRotationalTolerance: 0.025 * 5.0,
                                            armMoveTranslationalTolerance: 0.025,
                                            liftHeight: 0.140,
                                            defaultSubTaskStepTimeOutmSec: 1000,
                                            gripExchangePose: new Pose(new Vector3(-0.45, 0.57, 0.005), new Quaternion(0.0, 0.0, 0.7285, -0.6851)),
                                            defaultNeutralPose: new Pose(new Vector3(-0.61, 0.0, 0.356)));

            this.manipTimerHost = agentLocator.Register(this.manipTimerAgent);
            IAgentHost manipHost = agentLocator.Register(armAgent);
            this.gripperHost = agentLocator.Register(robotiqAgent);
            this.pickNPlaceSimulatorHost = agentLocator.Register(pickPlaceAgent);


            this.objectDetectorAgent = new MessageInjectorAgent<KinectObjectDetectorResultIRMessage>(objectDetectorName);
            this.visualIntegrationAgent = new ObjectDetectorPicknPlaceIntegratorAgent(
                                                                            integrationAgentName,
                                                                            pickPlaceAgent,
                                                                            objectDetectorName,
                                                                            string.Empty,
                                                                            minConfidence,
                                                                            maxRetry,
                                                                            minDetectionCount);

            this.PicknPlaceStatusConsumerAgent = new MessageCollectorAgent<PicknPlaceStatusMessage>("collector", this.visualIntegrationAgent);

            this.objectDetectorHost = agentLocator.Register(objectDetectorAgent);
            this.visualIntegrationHost = agentLocator.Register(visualIntegrationAgent);

            this.pickMoveMessageInjector = this.visualIntegrationHost.GetControlPort<PickAndMoveMessage>();
            this.placeMoveMessageInjector = this.visualIntegrationHost.GetControlPort<PlaceAndMoveMessage>();

            this.PicknPlaceConsumerHost = agentLocator.Register(this.PicknPlaceStatusConsumerAgent);

            this.visualIntegrationHost.Activate();
            this.gripperHost.Activate();
            this.PicknPlaceConsumerHost.Activate();
        }

        /// <summary>
        /// Shutdown the pipeline
        /// </summary>
        private void ShutDownPipeline()
        {
            if (null != this.PicknPlaceConsumerHost)
            {
                this.PicknPlaceConsumerHost.Deactivate();
            }

            if (null != this.visualIntegrationHost)
            {
                this.visualIntegrationHost.Deactivate();
            }

            if (null != this.gripperHost)
            {
                this.gripperHost.Deactivate();
            }
        }
    }
}
